addpath build
mathematica_path = ...
    'C:\Program Files\Wolfram Research\Mathematica\8.0\';
main_path = ...
    'C:\\Users\rsinnet\\Documents\\MATLAB\\meen-652-biped\\code\\';
st = load('traj.mat', 'traj');
traj = st.traj;

N = length(traj.t);
ndof = fix(size(traj.x, 1) / 2);

t = traj.t;
x0 = traj.x;
phip = traj.phip;

A = nan(2*ndof, 2*ndof, N-1);
B = nan(2*ndof, ndof, N-1);
C = nan(ndof, 2*ndof, N-1);
D = nan(ndof, ndof, N-1);

uff = nan(ndof, N-1);

K = nan(ndof, 2*ndof, N-1);
P = nan(2*ndof, 2*ndof, N);
Q = nan(2*ndof, 2*ndof, N-1);
R = nan(ndof, ndof, N-1);

xk = x0(:, end);
save('xk.mat', 'xk');
system(...
    ['"' mathematica_path 'math.exe" ' ...
    '-noprompt -run "<<' ...
    main_path 'doLinearization.nb; Quit[];']);
st = load('.\\build\\lsys\\ss.mat', 'A', 'B', 'C', 'D');
P(:, :, end) = st.C' * st.C;


for k = fliplr(1:N-1)
    xk = x0(:, k);
    dqk = xk(ndof+1:end);
    save('xk.mat', 'xk');
    system(...
        ['"' mathematica_path 'math.exe" ' ...
        '-noprompt -run "<<' ...
        main_path 'doLinearization.nb; Quit[];']);
    st = load('.\\build\\lsys\\ss.mat', 'A', 'B', 'C', 'D');
    A(:, :, k) = st.A;
    B(:, :, k) = st.B;
    C(:, :, k) = st.C;
    D(:, :, k) = st.D;
    
    Q(:, :, k) = C(:, :, k)' * C(:, :, k);
    R(:, :, k) = 1/100*eye(ndof);
    
    xD = D_mat(xk);
    xC = C_mat(xk);
    xG = G_vec(xk);
        
    % feedforward control gains (cancel out nonlinearities)
    uff(:, k) = xC * dqk + xG;
    
    
    K(:, :, k) = ...
        (B(:, :, k)'*P(:, :, k+1)*B(:, :, k) + R(:, :, k)) \ ...
        (B(:,:,k)'*P(:, :, k+1)*A(:, :, k));
    
    P(:, :, k) = A(:, :, k)' * P(:, :, k+1) * ...
        (A(:, :, k) - B(:, :, k)*K(:, :, k)) + Q(:, :, k);
    
    
    fprintf('Completion: %2.2f%%\n', 100*(N-k)/N);
end

save('traj_ss_lsys.mat',...
    't', 'phip', 'x0', ...
    'A', 'B', 'C', 'D', ...
    'K', 'uff');
